{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "8d37c838",
   "metadata": {},
   "source": [
    "## Here we will learn some basic instrcutions of panda_py"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "cf268831",
   "metadata": {},
   "outputs": [],
   "source": [
    "import panda_py\n",
    "from panda_py import libfranka\n",
    "\n",
    "panda = panda_py.Panda(\"172.16.0.2\")\n",
    "hand = libfranka.Gripper(\"172.16.0.2\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c3b00d81",
   "metadata": {},
   "source": [
    "### Instruction1: move the gripper"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0729c36c",
   "metadata": {},
   "outputs": [
    {
     "ename": "TypeError",
     "evalue": "move_to_pose(): incompatible function arguments. The following argument types are supported:\n    1. (self: panda_py._core.Panda, positions: list[numpy.ndarray[numpy.float64[3, 1]]], orientations: list[numpy.ndarray[numpy.float64[4, 1]]], speed_factor: float = 0.2, impedance: numpy.ndarray[numpy.float64[6, 6]] = array([[800., 0., 0., 0., 0., 0.], [ 0., 800., 0., 0., 0., 0.], [ 0., 0., 800., 0., 0., 0.], [ 0., 0., 0., 40., 0., 0.], [ 0., 0., 0., 0., 40., 0.], [ 0., 0., 0., 0., 0., 40.]]), damping_ratio: float = 1.0, nullspace_stiffness: float = 15.0, dq_threshold: float = 0.001, success_threshold: float = 0.01) -> bool\n    2. (self: panda_py._core.Panda, position: numpy.ndarray[numpy.float64[3, 1]], orientation: numpy.ndarray[numpy.float64[4, 1]], speed_factor: float = 0.2, impedance: numpy.ndarray[numpy.float64[6, 6]] = array([[800., 0., 0., 0., 0., 0.], [ 0., 800., 0., 0., 0., 0.], [ 0., 0., 800., 0., 0., 0.], [ 0., 0., 0., 40., 0., 0.], [ 0., 0., 0., 0., 40., 0.], [ 0., 0., 0., 0., 0., 40.]]), damping_ratio: float = 1.0, nullspace_stiffness: float = 15.0, dq_threshold: float = 0.001, success_threshold: float = 0.01) -> bool\n    3. (self: panda_py._core.Panda, pose: list[numpy.ndarray[numpy.float64[4, 4]]], speed_factor: float = 0.2, impedance: numpy.ndarray[numpy.float64[6, 6]] = array([[800., 0., 0., 0., 0., 0.], [ 0., 800., 0., 0., 0., 0.], [ 0., 0., 800., 0., 0., 0.], [ 0., 0., 0., 40., 0., 0.], [ 0., 0., 0., 0., 40., 0.], [ 0., 0., 0., 0., 0., 40.]]), damping_ratio: float = 1.0, nullspace_stiffness: float = 15.0, dq_threshold: float = 0.001, success_threshold: float = 0.01) -> bool\n    4. (self: panda_py._core.Panda, pose: numpy.ndarray[numpy.float64[4, 4]], speed_factor: float = 0.2, impedance: numpy.ndarray[numpy.float64[6, 6]] = array([[800., 0., 0., 0., 0., 0.], [ 0., 800., 0., 0., 0., 0.], [ 0., 0., 800., 0., 0., 0.], [ 0., 0., 0., 40., 0., 0.], [ 0., 0., 0., 0., 40., 0.], [ 0., 0., 0., 0., 0., 40.]]), damping_ratio: float = 1.0, nullspace_stiffness: float = 15.0, dq_threshold: float = 0.001, success_threshold: float = 0.01) -> bool\n\nInvoked with: <panda_py._core.Panda object at 0x7fde181209b0>, [0.3, 0.0, 0.4]",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mTypeError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[2], line 4\u001b[0m\n\u001b[1;32m      2\u001b[0m hand\u001b[38;5;241m.\u001b[39mmove(\u001b[38;5;241m0.08\u001b[39m,\u001b[38;5;241m0.2\u001b[39m)  \u001b[38;5;66;03m## release# Move the arm to a position (if orientation is optional)\u001b[39;00m\n\u001b[1;32m      3\u001b[0m target_position \u001b[38;5;241m=\u001b[39m [\u001b[38;5;241m0.3\u001b[39m, \u001b[38;5;241m0.0\u001b[39m, \u001b[38;5;241m0.4\u001b[39m]  \u001b[38;5;66;03m# x, y, z in meters\u001b[39;00m\n\u001b[0;32m----> 4\u001b[0m panda\u001b[38;5;241m.\u001b[39mmove_to_pose(target_position)\n",
      "\u001b[0;31mTypeError\u001b[0m: move_to_pose(): incompatible function arguments. The following argument types are supported:\n    1. (self: panda_py._core.Panda, positions: list[numpy.ndarray[numpy.float64[3, 1]]], orientations: list[numpy.ndarray[numpy.float64[4, 1]]], speed_factor: float = 0.2, impedance: numpy.ndarray[numpy.float64[6, 6]] = array([[800., 0., 0., 0., 0., 0.], [ 0., 800., 0., 0., 0., 0.], [ 0., 0., 800., 0., 0., 0.], [ 0., 0., 0., 40., 0., 0.], [ 0., 0., 0., 0., 40., 0.], [ 0., 0., 0., 0., 0., 40.]]), damping_ratio: float = 1.0, nullspace_stiffness: float = 15.0, dq_threshold: float = 0.001, success_threshold: float = 0.01) -> bool\n    2. (self: panda_py._core.Panda, position: numpy.ndarray[numpy.float64[3, 1]], orientation: numpy.ndarray[numpy.float64[4, 1]], speed_factor: float = 0.2, impedance: numpy.ndarray[numpy.float64[6, 6]] = array([[800., 0., 0., 0., 0., 0.], [ 0., 800., 0., 0., 0., 0.], [ 0., 0., 800., 0., 0., 0.], [ 0., 0., 0., 40., 0., 0.], [ 0., 0., 0., 0., 40., 0.], [ 0., 0., 0., 0., 0., 40.]]), damping_ratio: float = 1.0, nullspace_stiffness: float = 15.0, dq_threshold: float = 0.001, success_threshold: float = 0.01) -> bool\n    3. (self: panda_py._core.Panda, pose: list[numpy.ndarray[numpy.float64[4, 4]]], speed_factor: float = 0.2, impedance: numpy.ndarray[numpy.float64[6, 6]] = array([[800., 0., 0., 0., 0., 0.], [ 0., 800., 0., 0., 0., 0.], [ 0., 0., 800., 0., 0., 0.], [ 0., 0., 0., 40., 0., 0.], [ 0., 0., 0., 0., 40., 0.], [ 0., 0., 0., 0., 0., 40.]]), damping_ratio: float = 1.0, nullspace_stiffness: float = 15.0, dq_threshold: float = 0.001, success_threshold: float = 0.01) -> bool\n    4. (self: panda_py._core.Panda, pose: numpy.ndarray[numpy.float64[4, 4]], speed_factor: float = 0.2, impedance: numpy.ndarray[numpy.float64[6, 6]] = array([[800., 0., 0., 0., 0., 0.], [ 0., 800., 0., 0., 0., 0.], [ 0., 0., 800., 0., 0., 0.], [ 0., 0., 0., 40., 0., 0.], [ 0., 0., 0., 0., 40., 0.], [ 0., 0., 0., 0., 0., 40.]]), damping_ratio: float = 1.0, nullspace_stiffness: float = 15.0, dq_threshold: float = 0.001, success_threshold: float = 0.01) -> bool\n\nInvoked with: <panda_py._core.Panda object at 0x7fde181209b0>, [0.3, 0.0, 0.4]"
     ]
    }
   ],
   "source": [
    "hand.move(0.05,0.2)  ## the first parameter is the gripper width, the second is the speed\n",
    "hand.move(0.08,0.2)  ## max=0.08m\n",
    "target_position = [0.3, 0.0, 0.4]  # x, y, z in meters\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "56e5c6db",
   "metadata": {},
   "source": [
    "### Instruction2: use the gripper to grasp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "86adc6ab",
   "metadata": {},
   "outputs": [
    {
     "ename": "RuntimeError",
     "evalue": "libfranka: TCP send bytes: Timeout",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mRuntimeError\u001b[0m                              Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[41], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m success \u001b[38;5;241m=\u001b[39m hand\u001b[38;5;241m.\u001b[39mgrasp(\u001b[38;5;241m0.02\u001b[39m, \u001b[38;5;241m0.04\u001b[39m, \u001b[38;5;241m40\u001b[39m) \u001b[38;5;66;03m# Grasp the object with a force of 0.02, speed of 0.04, and timeout of 40 Newtons\u001b[39;00m\n\u001b[1;32m      2\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mResults:\u001b[39m\u001b[38;5;124m\"\u001b[39m, success)\n\u001b[1;32m      4\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mtime\u001b[39;00m\n",
      "\u001b[0;31mRuntimeError\u001b[0m: libfranka: TCP send bytes: Timeout"
     ]
    }
   ],
   "source": [
    "success = hand.grasp(0.02, 0.04, 40) # Grasp the object with a width of 0.02, speed of 0.04, and timeout of 40 Newtons\n",
    "print(\"Results:\", success)\n",
    "\n",
    "import time\n",
    "\n",
    "time.sleep(10)  # Hold the grasp for 10 seconds\n",
    "\n",
    "hand.move(0.08,0.2)  ## release"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5be0eace",
   "metadata": {},
   "source": [
    "### Instruction3: move the arm"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e28209f6",
   "metadata": {},
   "source": [
    "#### Here we will introduce move_to_pose function to realize the movement and rotation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "63189595",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 9.99968106e-01 -6.63087565e-03  7.52199094e-04  3.07474316e-01]\n",
      " [-6.63288285e-03 -9.99964735e-01  2.69812259e-03  8.02953703e-04]\n",
      " [ 7.34281653e-04 -2.70302579e-03 -9.99996077e-01  4.86071191e-01]\n",
      " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n"
     ]
    }
   ],
   "source": [
    "panda.move_to_start()\n",
    "\n",
    "print(panda.get_pose())  # Print the current pose of the robot"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "507df6ac",
   "metadata": {},
   "source": [
    "#### Move the arm to a position with the gripper always looking down"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "9b6a7231",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Current pose: [[ 0.98691085  0.12122521 -0.10626564  0.28921488]\n",
      " [ 0.13416337 -0.9830959   0.12451379  0.37253433]\n",
      " [-0.08937511 -0.13714096 -0.98651099  0.31379405]\n",
      " [ 0.          0.          0.          1.        ]]\n"
     ]
    }
   ],
   "source": [
    "target_position = [0.3, 0.4, 0.3]  # x, y, z in meters\n",
    "target_orientation = [1,0,0,0]  # Quaternion [w, z, y, x] for gripper down\n",
    "# Get and print the current pose of the robot\n",
    "\n",
    "panda.move_to_pose(target_position, target_orientation)\n",
    "current_pose = panda.get_pose()\n",
    "import numpy as np\n",
    "\n",
    "def print_pose_no_scientific(pose):\n",
    "    np.set_printoptions(suppress=True)\n",
    "    print(\"Current pose:\", pose)\n",
    "current_pose = panda.get_pose()\n",
    "print_pose_no_scientific(current_pose)\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8329cbe9",
   "metadata": {},
   "source": [
    "#### Make the end effector rotate 90 degrees"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "c59defd6",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Current pose: [[ 0.16542592  0.98240663 -0.08655846  0.39080201]\n",
      " [ 0.97489415 -0.17615507 -0.1361319   0.01376437]\n",
      " [-0.14898459 -0.06186559 -0.98690209  0.31144349]\n",
      " [ 0.          0.          0.          1.        ]]\n"
     ]
    }
   ],
   "source": [
    "target_position = [0.4, 0, 0.3]  # x, y, z in meters\n",
    "target_orientation = [0.7071,0.7071,0,0]  # Quaternion [w, z, y, x] for gripper down\n",
    "# Get and print the current pose of the robot\n",
    "\n",
    "panda.move_to_pose(target_position, target_orientation)\n",
    "current_pose = panda.get_pose()\n",
    "import numpy as np\n",
    "\n",
    "def print_pose_no_scientific(pose):\n",
    "    np.set_printoptions(suppress=True)\n",
    "    print(\"Current pose:\", pose)\n",
    "current_pose = panda.get_pose()\n",
    "print_pose_no_scientific(current_pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6912304a",
   "metadata": {},
   "source": [
    "#### Use matrix to rotate"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "29bc9552",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "panda.move_to_start()\n",
    "cur_pose = panda.get_pose()\n",
    "print(\"Current Pose:\", cur_pose)\n",
    "pose=panda.get_pose()\n",
    "\n",
    "theta=np.pi/4  # 45 degrees\n",
    "\n",
    "Rz=np.array([\n",
    "    [np.cos(theta), -np.sin(theta), 0],\n",
    "    [np.sin(theta), np.cos(theta), 0],\n",
    "    [0, 0, 1]\n",
    "    ])\n",
    "\n",
    "pose[:3,:3]=Rz @ pose[:3,:3]\n",
    "\n",
    "panda.move_to_pose(pose, dq_threshold=5e-4)\n",
    "cur_pose = panda.get_pose()\n",
    "print(\"Current Pose:\", cur_pose)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
